/*
 * @Date: 2020-10-20 01:41:30
 * @LastEditors: Tonser
 * @LastEditTime: 2020-10-20 13:50:54
 * @FilePath: /swarm_ws/src/px4_gcs/src/Utils/TrajManager.h
 * @Description: 工具类用于导入轨迹，从文件或者外部vector
 */
#pragma once

#include <vector>
#include <iostream>
#include "CSVManager.h"

template<class T>
class TrajManager
{
public:
    bool finished = false;
    vector<vector<T>> traj_scope; // [x,y,z][min,max]
    vector<vector<T>> traj;
    T trajLength;
    TrajManager(/* args */);
    ~TrajManager();
    void loadTrajFromVector(vector<vector<T>> &_traj);
    void loadTrajFromCSVFile(std::string filepath);
    void processTraj();
    static void resample(int num,vector<vector<T>> &trajectory);
};

template<class T>
TrajManager<T>::TrajManager(/* args */)
{

}

template<class T>
TrajManager<T>::~TrajManager()
{
    traj.clear();
}

template<class T> void
TrajManager<T>::loadTrajFromVector(vector<vector<T>> &_traj){
    traj.resize(_traj.size());
    traj.assign(_traj.begin(),_traj.end());
    finished = true;
}

template<class T> void
TrajManager<T>::processTraj() {
    vector<T> lastPoint = traj[0];
    traj_scope = vector<vector<T>>(3,vector<T>(2));
    trajLength = 0;
    for (int i = 0; i < traj.size(); ++i) {
        auto point = traj[i];
        // update scope
        traj_scope[0][0] = max(traj_scope[0][0],point[traj[0].size()-3]);
        traj_scope[0][1] = min(traj_scope[0][1],point[traj[0].size()-3]);
        traj_scope[1][0] = max(traj_scope[1][0],point[traj[0].size()-2]);
        traj_scope[1][1] = min(traj_scope[1][1],point[traj[0].size()-2]);
        traj_scope[2][0] = max(traj_scope[2][0],point[traj[0].size()-1]);
        traj_scope[2][1] = min(traj_scope[2][1],point[traj[0].size()-1]);
        //update traj
        trajLength += sqrt(
                (point[0]-lastPoint[0])*(point[0]-lastPoint[0])+
                (point[1]-lastPoint[1])*(point[1]-lastPoint[1])+
                (point[2]-lastPoint[2])*(point[2]-lastPoint[2]));
        // update last point
        lastPoint = point;
    }
}

// 从CSV文件中读取
template<class T> void
TrajManager<T>::loadTrajFromCSVFile(std::string filepath){
    CSVManager<T> fileManagaer(filepath);
    fileManagaer.readCSV(traj);
    finished = true;
}

// 把traj的轨迹点数由原来的size个减小为num个 num应该和size 差不多大
template<class T> void
TrajManager<T>::resample(int num,vector<vector<T>> &trajectory){
    vector<vector<T>> resampled_traj;
    resampled_traj.resize(num);
    int size = trajectory.size();
    // num 应该和size 差不多大
    if (size > num) {
        int delet_num = size - num;
        int set_num = (size - 2) / (delet_num);
        int mod_num = size - delet_num * set_num;
        int rindex = 0;
        int index = 0;
        resampled_traj[rindex++] = trajectory[index++];
        for (int j = 0; j < mod_num / 2; ++j) {
            resampled_traj[rindex++] = trajectory[index++];
        }

        for (int k = 0; k < delet_num; ++k) {
            for (int j = 0; j < set_num - 1; ++j) {
                resampled_traj[rindex++] = trajectory[index++];
            }
            index++;
        }
        for (int i = 0; i < (mod_num - mod_num / 2); ++i) {
            resampled_traj[rindex++] = trajectory[index++];
        }
        resampled_traj[num - 1] = trajectory[size - 1];
        trajectory = resampled_traj;
    }
}



